package org.apache.commons.graph.algorithms.shortestpath

import org.apache.commons.graph.DirectedGraph
import org.apache.commons.graph.Graph
import org.apache.commons.graph.Mapper
import org.apache.commons.graph.weight.WeightedPath
import org.apache.commons.graph.algorithms.shortestpath.exception.PathNotFoundException
import org.apache.commons.graph.algorithms.shortestpath.internal.ShortestDistances
import org.apache.commons.graph.collections.FibonacciHeap
import org.apache.commons.graph.weight.OrderedMonoid

/*
* Licensed to the Apache Software Foundation (ASF) under one
* or more contributor license agreements.  See the NOTICE file
* distributed with this work for additional information
* regarding copyright ownership.  The ASF licenses this file
* to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance
* with the License.  You may obtain a copy of the License at
*
*   http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing,
* software distributed under the License is distributed on an
* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
* KIND, either express or implied.  See the License for the
* specific language governing permissions and limitations
* under the License.
*/

/**
 *
 * @param <V> the Graph vertices type.
 * @param <WE> the Graph weighted edges type
 * @param <W> the weight type
</W></WE></V> */
interface HeuristicBuilder<V, WE, W> {
    /**
     *
     * @param heuristic
     * @param <H>
     * @return
    </H> */
    fun <H : Heuristic<V, W>> withHeuristic(heuristic: H): WeightedPath<V, WE, W>
}


/*
* Licensed to the Apache Software Foundation (ASF) under one
* or more contributor license agreements.  See the NOTICE file
* distributed with this work for additional information
* regarding copyright ownership.  The ASF licenses this file
* to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance
* with the License.  You may obtain a copy of the License at
*
*   http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing,
* software distributed under the License is distributed on an
* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
* KIND, either express or implied.  See the License for the
* specific language governing permissions and limitations
* under the License.
*/

internal class DefaultHeuristicBuilder<V, WE, W>(
    private val graph: Graph<V, WE>,
    private val weightedEdges: Mapper<WE, W>,
    private val start: V,
    private val goal: V,
    private val weightOperations: OrderedMonoid<W>
) : HeuristicBuilder<V, WE, W> {

    /**
     * {@inheritDoc}
     */
    override fun <H : org.apache.commons.graph.algorithms.shortestpath.Heuristic<V, W>> withHeuristic(heuristic: H): WeightedPath<V, WE, W> {
        //var heuristic = heuristic
        //heuristic = Assertions.checkNotNull(heuristic, "A* algorithm can not be applied using a null heuristic")

        // Cost from start along best known path.
        val gScores = ShortestDistances<V, W>(weightOperations)
        gScores.setWeight(start, weightOperations.identity())

        // Estimated total cost from start to goal through y.
        val fScores = ShortestDistances<V, W>(weightOperations)
        var hScore = heuristic.applyHeuristic(start, goal)
        fScores.setWeight(start, hScore)

        // The set of nodes already evaluated.
        val closedSet: MutableSet<V> = HashSet()

        // The set of tentative nodes to be evaluated.
        val openSet = FibonacciHeap(fScores)
        openSet.add(start)

        // The of navigated nodes
        val predecessors = org.apache.commons.graph.algorithms.shortestpath.PredecessorsList(
            graph,
            weightOperations,
            weightedEdges
        )

        // extract the node in openset having the lowest f_score[] value
        while (!openSet.isEmpty()) {
            val current = openSet.removeAndGet()!!

            // destination reached, stop and build the path
            if (goal == current) {
                return predecessors.buildPath(start, goal)
            }

            closedSet.add(current)

            val connected = if (graph is DirectedGraph<*, *>)
                (graph as DirectedGraph<V, WE>).getOutbound(current)
            else
                graph.getConnectedVertices(current)
            for (v in connected) {
                if (!closedSet.contains(v)) {
                    val edge = graph.getEdge(current, v)!!
                    // note that the weight of current can never be undefined
                    val tentativeGScore = weightOperations.append(gScores.getWeight(current)!!, weightedEdges.map(edge))

                    // if the first condition fails, v has already been visited (its weight is defined)
                    if (openSet.add(v) || weightOperations.compare(tentativeGScore, gScores.getWeight(v)) < 0) {
                        predecessors.addPredecessor(v, current)
                        gScores.setWeight(v, tentativeGScore)
                        hScore = heuristic.applyHeuristic(v, goal)
                        fScores.setWeight(v, weightOperations.append(gScores.getWeight(v)!!, hScore))
                    }
                }
            }
        }

        throw PathNotFoundException("Path from '%s' to '%s' doesn't exist in Graph '%s'", start, goal, graph)
    }
}
